import ctypes
import os
import sys
import time
import socket
import logging
import math


#   调用结构体 POSE
class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class SockCom():
    """
    sock的建立 发送 接受
    """

    def __init__(self,ip,port):

        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.setblocking(True)  #设置阻塞
        # 连接到目标主机和端口
        server_address = (ip, port)
        logging.info('connecting to %s port %s' % server_address)
        self.tag = self.sock.connect_ex(server_address)

        self.is_connect()

    def is_connect(self):

        if self.tag: #非0显示连接失败
            logging.error(f'socket连接失败:{self.tag}')

        else:
            logging.info(f'socket连接成功')



    def send(self,character):


        """
        character:给照相机发送的指令 发送指令 paizhao就会拍照
        """

        self.message = character
        logging.info('sending "%s"' % self.message)
        self.sock.sendall(self.message.encode())

    def recv(self):
        """

        :return: material,px,py  代表当前识别出的颜色的数字，物品在相机坐标系x轴的位置，物品在相机坐标系y轴的位置

        """

        # 接收返回值
        logging.info('开始拍照')
        data = self.sock.recv(1024)
        str2 = data.decode()



        while str2 == "00":
            logging.info(f'当前相机返回的信息：00')

            self.sock.send(self.message.encode('utf-8'))
            data = self.sock.recv(1024)
            str2 = data.decode()
            time.sleep(2)

        logging.info(f'当前相机返回的信息：{str2}')

        try:
            result = str2.split(',')

            if len(result) > 1:
                material = result[0]
                px = float(result[1])/1000 - 0.26024 #吸取基准点X
                py = float(result[2])/1000 + 0.145 #吸取基准点Y

                # px = float(result[1])/1000 - 0.26
                # py = float(result[2])/1000 + 0.15



                rz = float(result[3])* math.pi /180

                return [material, px, py, rz]

            else:

                return result[0]

        except Exception as e:
            logging.error(f'拍照处理结果出错：{e}')

    def close(self):

        self.sock.close()

class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "../RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')

    def run_paizhao(self):
        float_joint = ctypes.c_float * 6
        joint = float_joint(*[-39.433, 3.204, 87.278, -1.629, 89.719, -100.253])
        self.pDll.Movej_Cmd.argtypes = (ctypes.c_int, ctypes.c_float * 6, ctypes.c_byte,
                                        ctypes.c_float, ctypes.c_bool)

        self.pDll.Movej_Cmd.restype = ctypes.c_int
        ret = self.pDll.Movej_Cmd(self.nSocket, joint, 20, 0, 1)
        logging.info(f'机械臂运行至拍照位置：{ret}')


    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)


def main():
    sock = SockCom('192.168.1.66', 6000)
    ROT1 = Arm(ip="192.168.1.19")
    ROT1.run_paizhao()
    sock.send('paizhao')
    result = sock.recv()
    print(f'物料颜色及坐标：{result}')
    ROT1.close()
    sock.close()

if __name__ == "__main__":
    main()


